/**
	@file gps.cpp
	@brief GPS utility library.
	@details This Librarie is a bunch of tool to work with NMEA string, GPS coordinate and navigation data.
*/

#include "Arduino.h"
#include "gps.h"
#include "math.h"
#include "string.h"
#include "stdlib.h"



/**
	@fn gps::gps()
	Constructor that create an instance of gps()
*/
gps::gps()
{
	valid = VALID_VOID; // start with info taged as void
	GotZda = false;
}

/**
	@fn float gps::GetDist(float Lat1, float Lon1, float Lat2, float Lon2)
	@brief Get the distance between to coordinate.
	@details Get the distance in KM between 2 coordinate. The coordinate expected format is float Degree.Decimal.
	@param Lat1 Latitude of coordinate 1.
	@param Lon1 Longitude of coordinate 1.
	@param Lat2 Latitude of coordinate 2.
	@param Lon2 Longitude of coordinate 2.
	@return float Distance in Kilometters.
*/
float gps::GetDist(float Lat1, float Lon1, float Lat2, float Lon2)
{
	float DeltaLat = degreeToRadian(Lat2 - Lat1);
	float DeltaLon = degreeToRadian(Lon2 - Lon1);
	float Lat1Rad = degreeToRadian(Lat1);
	float Lat2Rad = degreeToRadian(Lat2);
	
	float a = sin(DeltaLat/2.0) * sin(DeltaLat/2.0) + 
		sin(DeltaLon/2.0) * sin(DeltaLon/2.0) * cos(Lat1Rad) * cos(Lat2Rad);

	return R * (2.0 * atan2(sqrt(a), sqrt(1.0-a)));
}

/**
	@fn int gps::GetBearing(float Lat1, float Lon1, float Lat2, float Lon2)
	@brief Get the bearing between 2 coordinate.
	@details Get the bearing in Degree between 2 coordinate. The coordinate expected format is float Degree.Decimal.
	@param Lat1 Latitude of coordinate 1.
	@param Lon1 Longitude of coordinate 1.
	@param Lat2 Latitude of coordinate 2.
	@param Lon2 Longitude of coordinate 2.
	@return float bearing in degree.
*/
int gps::GetBearing(float Lat1, float Lon1, float Lat2, float Lon2)
{
	float DeltaLon = degreeToRadian(Lon2 - Lon1);
	float Lat1Rad = degreeToRadian(Lat1);
	float Lat2Rad = degreeToRadian(Lat2);
	
	float y = sin(DeltaLon) * cos(Lat2Rad);
	float x = cos(Lat1Rad) * sin(Lat2Rad) -
		sin(Lat1Rad) * cos(Lat2Rad) * cos(DeltaLon);
	int brng = radianToDegree(atan2(y, x));
	return (brng + 360) % 360;
}

/**
	@fn int gps::ProcessNmeaStr(char *nmea, int Len)
	@brief Process an NMEA string.
	@details Process an NMEA string. The function expect a correctly formated NMEA string.
	@param nmea char array that contain de NMEA string
	@param Len int that represent the lenght of the NMEA string.
	@return int <0 on error, >0 on success. 
*/
int gps::ProcessNmeaStr(char *nmea, int Len)
{
	//Check if the string start with $ contain * and finish with \n
	if(nmea[0] != '$' || nmea[Len - 5] != '*' || nmea[Len - 1] != '\n') return NMEA_ERR_FORMAT;

#ifdef CHECKSUM_ENABLE
	else if(!CheckNmeaChecksum(nmea, Len)) return NMEA_ERR_CHECKSUM;
#endif	
	
	
	// type   time   V lat       lon        kts   crs   date   magvar  
	// $GPRMC,081836,A,3751.65,S,14507.36,E,000.0,360.0,130998,011.3,E*62 \r \n
	else if(strncmp(nmea, "$GPRMC", 6) == 0) 
	{
		ProcessNmea_RMC(nmea, Len);
		return NMEA_RMC;
	}

	
#ifdef PMTKCHN_ENABLE
	else if(strncmp(nmea, "$PMTKCHN", 8) == 0) 
	{
		ProcessNmea_PMTKCHN(nmea, Len);
		return NMEA_PMTKCHN;
	}
#endif	


#ifdef GPZDA_ENABLE
	else if(strncmp(nmea, "$GPZDA", 6) == 0) 
	{
		GotZda = true;
		ProcessNmea_ZDA(nmea, Len);
		return NMEA_GPZDA;
	}
#endif	
	return 0;
	
}
#ifdef GPZDA_ENABLE
/**
	@fn void gps::ProcessNmea_ZDA(char *nmea, int Len)
	@brief Process NMEA $GPZDA string.
	@details Used internaly by ProcessNmeaStr, this function process the NMEA $GPZDA string.
	GPZDA_ENABLE must be defined in gps_setup.h for this function to be included.
	The Time and Date in the $GPZDA string always take precedence on any other time&date data.
	@param nmea char array that contain de NMEA string
	@param Len int that represent the lenght of the NMEA string.
*/
void gps::ProcessNmea_ZDA(char *nmea, int Len)
{
	char *tok;
	int IDX = 0;
	
	tok = strtok(nmea, ","); // first Token is "$GPZDA"
	while(tok != NULL)
	{
		if(IDX == 1) nttots(tok, GpsInfo.time);
		else if(IDX == 2) GpsInfo.date.d = atoi(tok);
		else if(IDX == 3) GpsInfo.date.m = atoi(tok);
		else if(IDX == 4) GpsInfo.date.y = atoi(tok);
		else if(IDX == 5) break; // MUST ADD LOCAL OFFSET HERE.
		
		tok = strtok(NULL, ",");
		IDX++;
	}
}
#endif

#ifdef PMTKCHN_ENABLE
/**
	@fn void gps::ProcessNmea_PMTKCHN(char *nmea, int Len)
	@brief Process $PMTKCHN 'NMEA style' string.
	@details Used internaly by ProcessNmeaStr, this function process the MTK $PMTKCHN message.
	PMTKCHN_ENABLE must be defined in gps_setup.h for this function to be included.
	@param nmea char array that contain de NMEA string
	@param Len int that represent the lenght of the NMEA string.	
*/
void gps::ProcessNmea_PMTKCHN(char *nmea, int Len)
{
	char *tok;
	char tmp[3];
	tmp[2] = 0;
	int ChanIdx = -1;
	tok = strtok(nmea, ","); // first Token is "$PMTKCHN"
	while(tok != NULL && ChanIdx < TOTAL_CHANNEL)
	{
		if(ChanIdx != -1) // skip first token.
		{
			if(strlen(tok) < 5) // string to small to contain channel data
			{
				tok = strtok(NULL, ",");
				continue;
			}
			tmp[0] = tok[0];
			tmp[1] = tok[1];
			chan[ChanIdx].svid = atoi(tmp);
			
			tmp[0] = tok[2];
			tmp[1] = tok[3];
			chan[ChanIdx].snr = atoi(tmp);
			
			tmp[0] = tok[4];
			tmp[1] = 0;
			chan[ChanIdx].status = atoi(tmp);
		}
		ChanIdx++;
		tok = strtok(NULL, ",");
	}
}
#endif


/**
	@fn void gps::ProcessNmea_RMC(char *nmea, int Len)
	@brief Process $GPRMC NMEA string.
	@details Used internaly by ProcessNmeaStr, this function process the NMEA $GPRMC message.
	This function is always included and cannot be turned off in gps_setup.h
	@param nmea char array that contain de NMEA string
	@param Len int that represent the lenght of the NMEA string.	
*/
void gps::ProcessNmea_RMC(char *nmea, int Len)
{
	char *tok;
	RMCstr rmc;
	tok = strtok(nmea, ","); // first Token is "$GPRMC"
	
	rmc.time = strtok(NULL, ","); // time
	//sprintf(rmc.time, "%s", tok);
	
	tok = strtok(NULL, ","); // Valid
	if(tok[0] == 'A') rmc.valid = true;
	else rmc.valid = false;
	
	rmc.lat = strtok(NULL, ","); // Latitude
	
	tok = strtok(NULL, ","); // North South
	rmc.latNS = tok[0];
	
	rmc.lon = strtok(NULL, ","); // Longitude
	
	tok = strtok(NULL, ","); // East West
	rmc.lonEW = tok[0];
	
	tok = strtok(NULL, ","); // Knots
	rmc.knots = atof(tok);
	
	tok = strtok(NULL, ","); // Course
	rmc.course = atof(tok);

	rmc.date = strtok(NULL, ","); // Date
	
	tok = strtok(NULL, ","); // Magnetic Variation
	rmc.magvar = atof(tok);
	
	tok = strtok(NULL, ","); // MagVar EastWest
	rmc.magvarEW = tok[0];
	
	tok = strtok(NULL, ","); // Mode
	rmc.mode = tok[0];

	if(rmc.valid)
	{
		GpsInfo.latd = dmtodd(rmc.lat, rmc.latNS);
		GpsInfo.lond = dmtodd(rmc.lon, rmc.lonEW);
		
		GpsInfo.knots = rmc.knots;
		GpsInfo.course = rmc.course;
		
		if(rmc.magvarEW == 'W') GpsInfo.magvar = -abs(rmc.magvar);
		else GpsInfo.magvar = rmc.magvar;
		
		GpsInfo.mode = rmc.mode;
		if(!GotZda)
		{
			//if we never seen a $GPZDA string, set this time & date as GpsInfo .time .date
			ndtods(rmc.date, NMEA_RMC, GpsInfo.date);
			nttots(rmc.time, GpsInfo.time);
		}
		valid = VALID_ACTIVE;
	}
	else valid = VALID_VOID;
		
}


/**
	@fn void gps::getdatestr(char *date)
	@brief Format a date string in char *date
	@details This function will format a string that contain the date in char *date. Based on what NMEA message are enabled,
	the string will be YY-MM-DD or YYYY-MM-DD.
	@param date char array in wich the date will be formated.
*/
void gps::getdatestr(char *date)
{
	sprintf(date, "%02i-%02i-%02i", GpsInfo.date.y, GpsInfo.date.m, GpsInfo.date.d);
}


/**
	@fn boolean gps::ndtods(char *datestr, int fromtype, date_s &dateReturn)
	@brief NMEA RMC date string to date_s struct
	@details Used internaly, this function is used to set the date from an NMEA string in the date_s struct dateReturn.
	@param datestr NMEA date string.
	@param fromtype The type of NMEA date string (NMEA_RMC, NMEA_ZDA, ...)
	@param dateReturn The date_s struct to store the date in.
	@return true = valid convertion, false = error in convertion.
*/
boolean gps::ndtods(char *datestr, int fromtype, date_s &dateReturn)
{
	// DDMMYY or DDMMYYYY
	// 200512
	int YearIndex = 5; //default
	boolean ret = true;
	
	if(fromtype == NMEA_RMC) YearIndex = 5;
	//add other type here!
	else ret = false;
	
	char tmp[5];
	//date_s datetmp;
	
	int writeIdx = 0;
	int i;
	for(i = 0; i < strlen(datestr); i++)
	{
		if(writeIdx >= sizeof(tmp)) return false;
		tmp[writeIdx] = datestr[i];
		if(i == 1)
		{
			//Day
			writeIdx++;
			tmp[writeIdx] = 0;
			dateReturn.d = atoi(tmp);
			writeIdx = 0;
			continue;
		}
		else if(i == 3)
		{
			//Month
			writeIdx++;
			tmp[writeIdx] = 0;
			dateReturn.m = atoi(tmp);
			writeIdx = 0;
			continue;
		}
		else if(i == YearIndex)
		{
			//Year
			writeIdx++;
			tmp[writeIdx] = 0;
			dateReturn.y = atoi(tmp);
			writeIdx = 0;
			continue;
		}
		writeIdx++;
	}
	return ret;
}


/**
	@fn void gps::gettimestr(char *time)
	@brief Format a time string in char *time
	@details This function will format a string that contain the time in char *time. The string format will be HH:MM:SS.sss.
	@param time char array in wich the time will be formated.
*/
void gps::gettimestr(char *time)
{
	sprintf(time, "%02i:%02i:%02i.%03i", GpsInfo.time.h, GpsInfo.time.m, GpsInfo.time.s, GpsInfo.time.ms);
}


/**
	@fn boolean gps::nttots(char *timestr, time_s &timeReturn)
	@brief NMEA RMC time string to time_s struct
	@details Used internaly, this function is used to set the time in timeReturn time_s struct from an NMEA RMC time string.
	@param timestr NMEA RMC time string.
	@param timeReturn Then time_s struct to set the time in.
	@return bool true on convertion sucess, false on error.
*/
boolean gps::nttots(char *timestr, time_s &timeReturn)
{
	char tmp[6];
	time_s TimeTmp;
	
	int writeIdx = 0;
	//HHMMSS.sss
	//204013.000
	// 1 3 56  9
	int i;
	for(i = 0; i < strlen(timestr); i++)
	{
		if(i == 6) continue;
		
		tmp[writeIdx] = timestr[i];
		if(i == 1)
		{
			//Hrs
			writeIdx++;
			tmp[writeIdx] = 0;
			timeReturn.h = atoi(tmp);
			writeIdx = 0;
			continue;
		}
		else if(i == 3)
		{
			//Min
			writeIdx++;
			tmp[writeIdx] = 0;
			timeReturn.m = atoi(tmp);
			writeIdx = 0;
			continue;
		}
		else if(i == 5)
		{
			//Sec
			writeIdx++;
			tmp[writeIdx] = 0;
			timeReturn.s = atoi(tmp);
			writeIdx = 0;
			continue;
		}
		else if(i == 9)
		{
			//Mili Sec
			writeIdx++;
			tmp[writeIdx] = 0;
			timeReturn.ms = atoi(tmp);
			writeIdx = 0;
			continue;
		}
		
		writeIdx++;
	}
	
	if(i >= 9) return true;
	else return false;
}


/**
	@fn float gps::dmtodd(char *dm, char NSEW)
	@brief NMEA 'degree minute.m' to 'degree.decimal'
	@details Used to convert an NMEA style 'degree minute.m' string to a float 'degree.decimal'.
	The expected string format is:
		-LATITUDE:	ddmm.mm
		-LONGITUTE:	dddmm.mm
	@param dm string that contain the NMEA style 'degree minute.m' information.
	@param NSEW char the represent the cardinal direction of the dm string. Can be 'N' 'S' 'E' 'W'.
	@return float that represent the Degree.decimal. On error, return float maximum value ( __FLT_MAX__ )
*/
float gps::dmtodd(char *dm, char NSEW)
{
			char deg[6];
			char min[10];
			
			int mode = 0;
			int writeIdx = 0;
			
			int delim;
			
			if(NSEW == 'N' || NSEW == 'S') 
			{
				//ddmm.mmmm
				delim = 2;
			}
			else if(NSEW == 'E' || NSEW == 'W') 
			{
				//dddmm.mmmm
				delim = 3;
			}
			else return __FLT_MAX__;
			
			for(int i = 0; i < strlen(dm); i++)
			{
				//LAT
				if(i == delim) 
				{
					deg[writeIdx] = 0; // ending zero
					mode = 1;
					writeIdx = 0;
				}
				
				if(mode == 0)
				{
					deg[writeIdx] = dm[i];
				}
				else if(mode == 1)
				{
					min[writeIdx] = dm[i];
				}
				
				writeIdx++;

			}
			min[writeIdx] = 0; // ending zero

			if(NSEW == 'S' || NSEW == 'W') return -abs(atoi(deg) + (atof(min) / 60.0));
			return (atof(deg) + (atof(min) / 60.0));
}

/**
	@fn float gps::degreeToRadian(float degree)
	@brief convert degree to radian.
	@param degree the degree to convert in radia.
	@return float radian
*/
float gps::degreeToRadian(float degree)
{
        float radian = 0.0;
        radian = degree * (pi/180.0);
        return radian;
}

/**
	@fn float gps::radianToDegree(float radian)
	@brief Convert Radian to Degree.
	@param radian The radian to convert in degree.
	@return float degree.
*/
float gps::radianToDegree(float radian)
{
        float degree = 0.0;
        degree = radian * (180.0/pi);
        return degree;
}

/**
	@fn float gps::ktstokmh(float kts)
	@brief Convert Knots to Kilometers per Hour
	@param kts the Knots to convert in KM/H
	@return float KM/H
*/
float gps::ktstokmh(float kts)
{
	return (kts * 1.85200);
}

/**
	@fn float gps::ktstomph(float kts)
	@brief Convert Knots to Miles Per Hour
	@param kts The Knots to convert to MPH
	@return float MPH
*/
float gps::ktstomph(float kts)
{
	return (kts * 1.15077945);
}


#ifdef CHECKSUM_ENABLE

/**
	@fn boolean gps::CheckNmeaChecksum(char *nmea, int Len)
	@brief Check NMEA Checksum.
	@details Confirm that the checksum on a given NMEA string is valid. This Function is only included if CHECKSUM_ENABLE is defined in gps_setup.h
	@param nmea The NMEA string to confirm the checksum.
	@param Len The lenght of the NMEA string.
	@return boolean true = checksum valid, false = checksum failed.
*/
boolean gps::CheckNmeaChecksum(char *nmea, int Len)
{	
	int checksum = getCheckSum(nmea);
	if(checksum > 0xFF || checksum < 0x0) return false;
	char tmp[3];
	sprintf(tmp, "%02X", checksum);
	if(tmp[0] != nmea[Len - 4] || tmp[1] != nmea[Len - 3]) return false;
	return true;
}


// Calculates the checksum for a given string
// returns as integer
/**
	@fn int gps::getCheckSum(char *string)
	@brief Get checksum
	@details Get the Checksum value for a given NMEA string.
	@param string The NMEA string to generate a Checksum from.
	@return int Checksum
*/
int gps::getCheckSum(char *string)
{
	int i;
	int XOR = 0;
	int c;
	// Calculate checksum ignoring any $'s in the string
	for (i = 0; i < strlen(string); i++) 
	{
		c = (unsigned char)string[i];
		if (c == '*') break;
		if (c != '$') XOR ^= c;
	}
	return XOR;
}
#endif

 
